//
// Created by Wirano on 2020/11/9.
//

#include "Ano_Filter.h"
#include "Ano_Math.h"


/**
 * 整数滤波器
 * @param dT
 * @param data
 */
void inte_fix_filter(float dT,_inte_fix_filter_st *data)
{
    float ei_lim_val;

    if(data->ei_limit>0)
    {
        ei_lim_val = LIMIT(data->ei,-data->ei_limit,data->ei_limit);
    }
    else
    {
        ei_lim_val = data->ei;
    }

    data->out = (data->in_est + ei_lim_val);

    data->e = data->fix_ki *(data->in_obs - data->out);

    data->ei += data->e *dT;
}

/**
 * 固定点滤波器
 * @param dT
 * @param data
 */
void fix_inte_filter(float dT,_fix_inte_filter_st *data)
{

    data->out += (data->in_est_d + data->e ) *dT;

    data->e = data->fix_kp *(data->in_obs - data->out);

    if(data->e_limit>0)
    {
        data->e = LIMIT(data->e,-data->e_limit,data->e_limit);
    }


}



void limit_filter(float T,float hz,_lf_t *data,float in) //增量滤波，适合大噪声低滞后（无缝），收敛最快
{
    float abs_t;
// 	LPF_1(hz,T,	 in,&(data->lpf_1)); 
// 	abs_t = ABS(data->lpf_1);
// 	data->out = LIMIT(in,-abs_t,abs_t);
    LPF_1_(hz,T,	 in,(data->lpf_1));
    abs_t = ABS(in);
    data->out = LIMIT((data->lpf_1),-abs_t,abs_t);
}
void limit_filter_2(float T,float hz,_lf_t *data,float in) //增量滤波，均衡，但滞后稍大(数据反向等待低通反向)
{
    float abs_t;
    LPF_1_(hz,T,	 in,(data->lpf_1));
    abs_t = ABS(data->lpf_1);
    data->out = LIMIT(in,-abs_t,abs_t);
// 	LPF_1(hz,T,	 in,&(data->lpf_1)); 
//  	abs_t = ABS(in);
// 	data->out = LIMIT((data->lpf_1),-abs_t,abs_t);
}

void limit_filter_3(float T,float hz,_lf_t *data,float in) //增量滤波，适合低噪声滞后较小(等幅大噪声平行不收敛),噪声大收敛过慢
{
    float abs_t;
    LPF_1_(hz,T,	 in,(data->lpf_1));
    abs_t = ABS(in);
    data->out = data->lpf_1 = LIMIT((data->lpf_1),-abs_t,abs_t);

}

// #define STEEPEST_ARR_NUM 10
// #define STEEPEST_STEP 10  //次


void steepest_descend(int32_t arr[],uint8_t len,_steepest_st *steepest,uint8_t step_num,int32_t in)
{
    uint8_t updw = 1;//0 dw,1up
    int16_t i;
    uint8_t step_cnt=0;
    uint8_t step_slope_factor=1;
    uint8_t on = 1;
    int8_t pn = 1;
    //float last = 0;
    float step = 0;
    int32_t start_point = 0;
    int32_t pow_sum = 0;

    steepest->lst_out = steepest->now_out;

    if( ++(steepest->cnt) >= len )
    {
        (steepest->cnt) = 0; //now
    }

    //last = arr[ (steepest->cnt) ];

    arr[ (steepest->cnt) ] = in;

    step = (float)(in - steepest->lst_out)/step_num ;//梯度

    if(ABS(step)<1)//整形数据<1的有效判定
    {
        if(ABS(step)*step_num<2)
        {
            step = 0;
        }
        else
        {
            step = (step > 0) ? 1 : -1;
        }
    }

    start_point = steepest->lst_out;
    do
    {
        //start_point = steepest->lst_out;
        for(i=0;i<len;i++)
        {
// 			j = steepest->cnt + i + 1;
// 			if( j >= len )	
// 			{
// 				j = j - len; //顺序排列
// 			}
            pow_sum += my_pow(arr[i] - start_point );// /step_num;//除法减小比例**

            //start_point += pn *(step_slope_factor *step/len);
        }

        if(pow_sum - steepest->lst_pow_sum > 0)
        {
            if(updw==0)
            {
                on = 0;
            }
            updw = 1;//上升了
            pn = (pn == 1 )? -1:1;

        }
        else
        {
            updw = 0; //正在下降
            if(step_slope_factor<step_num)
            {
                step_slope_factor++;
            }
        }

        steepest->lst_pow_sum = pow_sum;
        pow_sum = 0;
        start_point += pn *step;//调整

        if(++step_cnt > step_num)//限制计算次数
        {
            on = 0;
        }
        //////
        if(step_slope_factor>=2)//限制下降次数1次，节省时间，但会增大滞后，若cpu时间充裕可不用。
        {
            on = 0;

        }
        //////

    }
    while(on==1);

    steepest->now_out = start_point ;//0.5f *(start_point + steepest->lst_out);//

    steepest->now_velocity_xdt = steepest->now_out - steepest->lst_out;
}





void fir_arrange_filter(float *arr,uint16_t len,uint8_t *fil_cnt,float in,float *arr_out) //len<=255 len >= 3
{
    //float arrange[len];
    float tmp;
    uint8_t i,j;
/*
窗口数据处理
*/
    if( ++*fil_cnt >= len )
    {
        *fil_cnt = 0; //now
    }

    arr[ *fil_cnt ] = in;
/*
窗口数据处理
*/

/*
赋值、排列
*/
    for(i=0;i<len;i++)
    {
        arr_out[i] = arr[i];
    }

    for(i=0;i<len-1;i++)
    {
        for(j=0;j<len-1-i;j++)
        {
            if(arr_out[j]>arr_out[j+1])
            {
                tmp = arr_out[j+1];
                arr_out[j+1] = arr_out[j];
                arr_out[j] = tmp;
            }
        }
    }
/*
赋值、排列
*/


}

// #define WIDTH_NUM 101
// #define FIL_ITEM  10

void Moving_Average(float moavarray[],uint16_t len ,uint16_t *fil_cnt,float in,float *out)
{
    uint16_t width_num;
    float last;

    width_num = len ;

    if( ++*fil_cnt >= width_num )
    {
        *fil_cnt = 0; //now
    }

    last = moavarray[ *fil_cnt ];

    moavarray[ *fil_cnt ] = in;

    *out += ( in - ( last  ) )/(float)( width_num ) ;
    //*out += 0.00001f *(in - *out);
    *out += 0.00001f *LIMIT((in - *out),-1,1);  //数据精度误差修正

}

void LPF_1(float hz,float time,float in,float *out)
{
    *out += ( 1 / ( 1 + 1 / ( hz *6.28f *time ) ) ) *( in - *out );

}

void LPF_1_db(float hz,float time,double in,double *out)
{
    *out += ( 1 / ( 1 + 1 / ( hz *6.28f *time ) ) ) *( in - *out );

}


void step_filter(float step,float in,float *out)
{
    if(in - *out > step)
    {
        *out += step;
    }
    else if(in - *out < -step)
    {
        *out -= step;
    }
    else
    {
        *out = in;
    }

}

float my_hpf_limited(float T,float hz,float x,float zoom,float *zoom_adj)
{


    *zoom_adj += ( 1 / ( 1 + 1 / ( hz *6.28f *T ) ) ) *(x - *zoom_adj);
    *zoom_adj = LIMIT(*zoom_adj,-zoom,zoom);
    return (x - *zoom_adj);

}


void simple_3d_trans(float ref[VEC_XYZ], float in[VEC_XYZ], float out[VEC_XYZ]) //该函数只有在水平面附近一个有限的范围内正确。
{
    static int8_t pn;
    static float h_tmp_x,h_tmp_y;

    h_tmp_x = my_sqrt(my_pow(ref[Z]) + my_pow(ref[Y]));
    h_tmp_y = my_sqrt(my_pow(ref[Z]) + my_pow(ref[X]));

    pn = ref[Z] < 0? -1 : 1;

    out[X] = ( h_tmp_x *in[X] - pn *ref[X] *in[Z] ) ;
    out[Y] = ( pn *h_tmp_y *in[Y] - ref[Y] *in[Z] ) ;

// 	 out[X] = h_tmp_x *in[X] - ref[X] *in[Z];
// 	 out[Y] = ref[Z] *in[Y] - ref[Y] *in[Z];

    out[Z] = ref[X] *in[X] + ref[Y] *in[Y] + ref[Z] *in[Z] ;

}

void vec_3dh_transition(float ref[VEC_XYZ], float in[VEC_XYZ], float out[VEC_XYZ])
{
    simple_3d_trans(ref,in,out); //
}

void vec_3dh_transition_matrix(float ref[VEC_XYZ],float wh_matrix[VEC_XYZ][VEC_XYZ])
{

}


//#define M_PI_F 3.14159f
//#include "math.h"
//void init_low_pass2_fliter(float sample_freq, float cutoff_freq,filter_s* imu_filter)
//{
//	imu_filter->_cutoff_freq = cutoff_freq;
//	
//	float fr = sample_freq/imu_filter->_cutoff_freq;
//	float ohm = tanf(M_PI_F/fr);
//	float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
//	imu_filter->_b0 = ohm*ohm/c;
//	imu_filter->_b1 = 2.0f*imu_filter->_b0;
//	imu_filter->_b2 = imu_filter->_b0;
//	imu_filter->_a1 = 2.0f*(ohm*ohm-1.0f)/c;
//	imu_filter->_a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
//	imu_filter->_delay_element_1 = 0.0;
//	imu_filter->_delay_element_2 = 0.0;
//}


//float low_pass2_filter(float sample,filter_s* imu_filter)//低通二阶滤波器
//{
//	float delay_element_0 = sample - imu_filter->_delay_element_1 * imu_filter->_a1 - imu_filter->_delay_element_2 * imu_filter->_a2;
//	float output = delay_element_0 * imu_filter->_b0 + imu_filter->_delay_element_1 * imu_filter->_b1 + imu_filter->_delay_element_2 * imu_filter->_b2;

//	imu_filter->_delay_element_2 = imu_filter->_delay_element_1;
//	imu_filter->_delay_element_1 = delay_element_0;

//	// return the value.  Should be no need to check limits
//	return output;
//}

